# read_xyz_fast.py
# pip install pyserial
import sys, time, argparse
import serial

REQ_ALL = bytes([0xFE, 0x04, 0x00, 0x00, 0x00, 0x08, 0xE5, 0xC3])  # 读 1~8 通道

def modbus_crc16(data: bytes) -> int:
    """标准Modbus RTU CRC16（返回0xFFFF内的整数；低字节在前）。"""
    crc = 0xFFFF
    for b in data:
        crc ^= b
        for _ in range(8):
            if crc & 1:
                crc = (crc >> 1) ^ 0xA001
            else:
                crc >>= 1
    return crc & 0xFFFF

def u16s_to_s16(hi: int, lo: int) -> int:
    v = (hi << 8) | lo
    if v >= 0x8000:
        v -= 0x10000
    return v

def read_exact(ser: serial.Serial, buf: bytearray, n: int, timeout_s: float) -> int:
    """尽快凑满n字节，带总超时。"""
    mv = memoryview(buf)[:n]
    got, t0 = 0, time.monotonic()
    while got < n:
        r = ser.readinto(mv[got:])
        if r:
            got += r
        else:
            # 无数据时检查超时
            if (time.monotonic() - t0) > timeout_s:
                break
    return got

def main():
    ap = argparse.ArgumentParser()
    ap.add_argument("--port", default="/dev/ttyACM0")
    ap.add_argument("--baud", type=int, default=38400)
    ap.add_argument("--display-hz", type=float, default=0.0, help="显示频率；设为0表示每帧都打印")
    ap.add_argument("--no-crc", action="store_true", help="跳过CRC校验以再快一点（不推荐在干扰环境）")
    ap.add_argument("--scale", type=float, default=0.01, help="原始int16到物理量的比例系数")
    ap.add_argument("--map", type=str, default="1,2,3", help="X,Y,Z对应的通道(1-8)，如 '2,5,6'")
    args = ap.parse_args()

    # 解析通道映射
    try:
        chx, chy, chz = [int(x.strip()) for x in args.map.split(",")]
        assert 1 <= chx <= 8 and 1 <= chy <= 8 and 1 <= chz <= 8
    except Exception:
        print("通道映射格式错误，应如 '1,2,3' 且范围1-8", file=sys.stderr)
        sys.exit(2)

    # 打开串口：尽量少阻塞 + 低延迟读取
    ser = serial.Serial(
        port=args.port,
        baudrate=args.baud,
        bytesize=serial.EIGHTBITS,
        parity=serial.PARITY_NONE,
        stopbits=serial.STOPBITS_ONE,
        timeout=0.02,              # 总读超时（配合 read_exact 再控）
        inter_byte_timeout=0.002,  # 字节间超时：一帧结束更快返回
        write_timeout=0.02
    )

    # 清空缓冲
    ser.reset_input_buffer()
    ser.reset_output_buffer()

    buf = bytearray(21)
    last_show = 0.0
    show_interval = (1.0 / args.display_hz) if args.display_hz > 0 else 0.0
    cnt, t_start = 0, time.monotonic()

    # 预计算各通道在resp中的字节索引
    def idx_pair(ch):  # ch: 1..8
        base = 3 + (ch - 1) * 2
        return base, base + 1

    ixh, ixl = idx_pair(chx)
    iyh, iyl = idx_pair(chy)
    izh, izl = idx_pair(chz)

    print(f"Reading {args.port} @ {args.baud} (8N1); X/Y/Z=ch{chx}/ch{chy}/ch{chz};显示Hz={args.display_hz}；按 Ctrl+C 退出")
    try:
        while True:
            # 发请求
            ser.write(REQ_ALL)

            # 读回固定 21 字节应答：addr(1)+func(1)+bytecnt(1=0x10)+data(16)+crc(2)
            got = read_exact(ser, buf, 21, timeout_s=0.05)
            if got != 21:
                # 丢帧：清输入缓冲，重来
                ser.reset_input_buffer()
                continue

            # 基本头检查
            if buf[1] != 0x04 or buf[2] != 0x10:
                continue

            # CRC（可选）
            if not args.no_crc:
                calc = modbus_crc16(buf[:19])
                lo = calc & 0xFF
                hi = (calc >> 8) & 0xFF
                if buf[19] != lo or buf[20] != hi:
                    continue  # 校验失败丢弃

            # 解析 X/Y/Z
            X = u16s_to_s16(buf[ixh], buf[ixl]) * args.scale
            Y = u16s_to_s16(buf[iyh], buf[iyl]) * args.scale
            Z = u16s_to_s16(buf[izh], buf[izl]) * args.scale

            cnt += 1
            now = time.monotonic()
            if show_interval == 0.0 or (now - last_show) >= show_interval:
                elapsed = now - t_start
                rate = cnt / elapsed if elapsed > 0 else 0.0
                # 打印一行，尽量简短
                print(f"{elapsed:8.3f}s  X={X:7.2f}  Y={Y:7.2f}  Z={Z:7.2f}   {rate:6.1f} fps")
                last_show = now

    except KeyboardInterrupt:
        pass
    finally:
        ser.close()

if __name__ == "__main__":
    main()
